#include <iostream>
#include "linear_system.h"

namespace auto_ros
{
namespace control
{
LinearSys::LinearSys(uint state_dim,
					 uint control_dim, uint distur_dim, uint out_dim)
	: state_dim_(state_dim), control_dim_(control_dim), distur_dim_(distur_dim), out_dim_(out_dim)
{
	state_matrix_ = Eigen::MatrixXd::Zero(state_dim, state_dim);
	control_matrix_ = Eigen::MatrixXd::Zero(state_dim, control_dim);
	distur_matrix_ = Eigen::MatrixXd::Zero(state_dim, distur_dim);
	out_state_matrix_ = Eigen::MatrixXd::Zero(out_dim, state_dim);
	out_distur_matrix_ = Eigen::MatrixXd::Zero(out_dim, distur_dim);
}
bool LinearSys::set_state_matrix(const Eigen::MatrixXd state_matrix)
{

	if ((state_matrix.rows() == state_dim_) && (state_matrix.cols() == state_dim_))
	{
		state_matrix_ = state_matrix;
		is_state_matrix_changed_ = true;
		return true;
	}
	else
	{
		//\033[31m red color cout,\033[0m defualt
		std::cout << "\033[31m Error:set state_matrix_ failed,state_matrix dim is not equal to state_dim \033[0m" << std::endl;
		return false;
	}
}
bool LinearSys::set_control_matrix(const Eigen::MatrixXd control_matrix)
{

	if ((control_matrix.rows() == state_dim_) && (control_matrix.cols() == control_dim_))
	{
		control_matrix_ = control_matrix;
		is_control_matrix_changed_ = true;
		return true;
	}
	else
	{
		std::cout << "\033[31m Error:set control_matrix_ failed,control_matrix dim is not equal to state_dim and control_dim \033[0m"
				  << std::endl;
		return false;
	}
}
bool LinearSys::set_distur_matrix(const Eigen::MatrixXd distur_matrix)
{
	if ((distur_matrix.rows() == state_dim_) && (distur_matrix.cols() == distur_dim_))
	{
		distur_matrix_ = distur_matrix;
		is_distur_matrix_changed_ = true;
		return true;
	}
	else if ((distur_matrix.cols() == distur_dim_) && (distur_matrix.cols() == 0))
	{
		distur_matrix_ = distur_matrix;
		return true;
	}
	else
	{
		std::cout << "\033[31m Error:set distur_matrix_ failed,distur_matrix dim is not equal to state_dim and distur_dim \033[0m"
				  << std::endl;
		return false;
	}
}
bool LinearSys::set_out_state_matrix(const Eigen::MatrixXd out_state_matrix)
{
	if ((out_state_matrix.rows() == out_dim_) && (out_state_matrix.cols() == state_dim_))
	{
		out_state_matrix_ = out_state_matrix;
		return true;
	}
	else
	{
		std::cout << "\033[31m Error:set set_out_state_matrix failed \033[0m"
				  << std::endl;
		return false;
	}
}
bool LinearSys::set_out_distur_matrix(const Eigen::MatrixXd out_distur_matrix)
{
	if ((out_distur_matrix.rows() == out_dim_) && (out_distur_matrix.cols() == distur_dim_))
	{
		out_distur_matrix_ = out_distur_matrix;
		return true;
	}
	else
	{
		std::cout << "\033[31m Error:set set_out_distur_matrix failed \033[0m"
				  << std::endl;
		return false;
	}
}
bool LinearSys::set_matrix(const Eigen::MatrixXd state_matrix,
						   const Eigen::MatrixXd control_matrix,
						   const Eigen::MatrixXd distur_matrix)
{
	bool temp_flag = set_state_matrix(state_matrix);
	temp_flag &= set_control_matrix(control_matrix);
	temp_flag &= set_distur_matrix(distur_matrix);
	return temp_flag;
}
Eigen::MatrixXd LinearSys::state_matrix()
{
	return state_matrix_;
}
Eigen::MatrixXd LinearSys::control_matrix()
{
	return control_matrix_;
}
Eigen::MatrixXd LinearSys::distur_matrix()
{
	return distur_matrix_;
}
Eigen::MatrixXd LinearSys::out_state_matrix()
{
	return out_state_matrix_;
}
Eigen::MatrixXd LinearSys::out_distur_matrix()
{
	return out_distur_matrix_;
}
uint LinearSys::control_dim()
{
	return control_dim_;
}
uint LinearSys::state_dim()
{
	return state_dim_;
}
uint LinearSys::distur_dim()
{
	return distur_dim_;
}
uint LinearSys::out_dim()
{
	return out_dim_;
}
bool LinearSys::is_state_matrix_changed()
{
	return is_state_matrix_changed_;
}
bool LinearSys::is_control_matrix_changed()
{
	return is_control_matrix_changed_;
}
bool LinearSys::is_distur_matrix_changed()
{
	return is_distur_matrix_changed_;
}

} // namespace control
} // namespace auto_ros
